// The MPC formulation for r^2, m^2 and h, similar to the one in Ni Dong's dissertation.
// One decision variable, T(t_k)

#include "MPC_EW2D_PatternedW.h"

/*
const double k_w = 3.3829e-12;
const double a_w = 0.6042;
const double E_w = 2.7e-3; // Unit = EV
const double k_c = 1.0274e-13;
const double a_c = 0.1669;
const double E_c = 1.9e-3; // Unit = EV
const double a_v = 15.55493;
const double k_v = 20.64504;
const double a_t = 0.02332;
const double k_t = 0.0261;
const double k_B = 8.617343e-5; // Boltzmann's constant, Unit = ev/K
*/
//const double k_w = 1.678251e-12;
//const double a_w = 6.194597e-01;
//const double E_w = 2.835225e-03;		// Unit = EV
//const double k_c = 4.399286e-17;
//const double a_c = 5.604130e-01;
//const double E_c = 1.624391e-03;		// Unit = EV
//const double a_t = 0.02332;
//const double k_t = 0.0261;
//const double a_v = 15.55493;
//const double k_v = 20.64504;
//const double k_B = 8.617343e-5; // Boltzmann's constant, Unit = ev/K
//const double T0_W0 = -6.581760e-01;
//const double T0_W1 = 5.178483e-01;
//const double T1_W0 = 3.105538e-03;
//const double T1_W1 = -2.409770e-03;
//const double T2_W0 = -4.879777e-06;
//const double T2_W1 = 3.741534e-06;
//const double T3_W0 = 2.553569e-09;
//const double T3_W1 = -1.937348e-09;


// -----------------------------------------------------------------------
/* Constructor. */
MPC1_PatternedW::MPC1_PatternedW():MPC_Formulation(4,4){
  int m,n;
  // Input parameters ---------------
  T_ini = 600;
  W_ini = 1.0;
  A_ini = 0.5;
  T_old = T_ini;
  W_old = W_ini;
  A_old = A_ini;
  Freq  = 5;

  // Output parameters -------------
  num_decision_val = 3;

  // Cost function -----------------
  r2_set = 30*30;
  m2_set = 0.2*0.2;
  q_r    = 0.0/(r2_set*r2_set);
  q_m    = 1.0/(m2_set*m2_set);

  // Constraints -------------------
  T_min = 500;
  T_max = 700;
  W_min = 0.1;
  W_max = 2.0;
  dT_max = 10; 
  dW_max = 0.5;
  A_min = 0.0;
  A_max = 2e19;

  // Model parameters --------------
  LatticeSize = 100;
  // -- Domain ---------------------
  XYmax  = 8000;
  t_end = 100.0;
  dt    = 5.0;
  mode  = 50;

  // 
  eq_f cost_function;
  int p;
  for(p=0;p<4;p++){
    cov_z_m[p]  = new double[(1+mode)*(1+mode)];
    mean_z_m[p] = new double[(1+mode)*(1+mode)];
    cov_z_p[p]  = new double[(1+mode)*(1+mode)];
    mean_z_p[p] = new double[(1+mode)*(1+mode)];
    dz_square_dT[p] = new double[(1+mode)*(1+mode)];
    dz_square_dW[p] = new double[(1+mode)*(1+mode)];
    dz_square_dA[p] = new double[(1+mode)*(1+mode)];
  }

  Km1 = new double*[mode+1];
  Km2 = new double*[mode+1];
  Km3 = new double*[mode+1];
  Km4 = new double*[mode+1];

  double dx = XYmax/LatticeSize;
  double temp;
  for(m=0;m<=mode;m++){
    Km1[m] = new double[mode+1];
    Km2[m] = new double[mode+1];
    Km3[m] = new double[mode+1];
    Km4[m] = new double[mode+1];

    // FIXME
    for(n=0;n<=mode;n++){

      temp =  4.0/(XYmax*XYmax)*sin(pi*m/LatticeSize)*sin(pi*m/LatticeSize);
      if(m==0||2*m==LatticeSize||n==0||2*n==LatticeSize){
	Km1[m][n] = 0.0;
      }
      else{
	Km1[m][n] = temp;
      }
      
      if(m==0){
	Km2[m][n] = 0.0;
      }
      else{
	Km2[m][n] = temp;
      }
      
      if(m==0||2*m==LatticeSize){
	Km3[m][n] = 0.0;
      }
      else{
	Km3[m][n] = temp;
      }

      if(n==0 || 2*n==LatticeSize || m==0){
	Km4[m][n] = 0.0;
      }
      else{
	Km4[m][n] = temp;
      }
    }
  }
  reset();
}
// -----------------------------------------------------------------------
MPC1_PatternedW::~MPC1_PatternedW(){
  int p;
  for(p=0;p<4;p++){
    delete []cov_z_m[p];
    delete []mean_z_m[p];
    delete []cov_z_p[p];
    delete []mean_z_p[p];
    delete []dz_square_dT[p];
    delete []dz_square_dW[p];
    delete []dz_square_dA[p];
  }
  int m;
  for(m=0;m<=mode;m++){
    delete [] Km1[m];
    delete [] Km2[m];
    delete [] Km3[m];
    delete [] Km4[m];
  }
  delete [] Km1;
  delete [] Km2;
  delete [] Km3;
  delete [] Km4;
}

// -----------------------------------------------------------------------
void MPC1_PatternedW::parameterize(double CurrentTime, double** inputs){
  int    p,m,n;
  int    index;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      index = index2D(m,n);
      for(p=0;p<4;p++){
	mean_z_m[p][index] = inputs[p][index];
	cov_z_m[p][index]  = mean_z_m[p][index]*mean_z_m[p][index]; // FIXME
      }
    }
  }
  time = CurrentTime;
  dt = t_end - CurrentTime;
}
// -----------------------------------------------------------------------
void MPC1_PatternedW::reset(){
  T_old = T_ini;
  W_old = W_ini;
  A_old = A_ini;
}
// -----------------------------------------------------------------------
void MPC1_PatternedW::setPara(int para_id,double para_val){
  // Put those tunable parameters here. For those un-tunable ones,
  // hard-code them in a derived class.
  assert(para_id < MPC_Formulation::num_para);
  switch(para_id){
  case 0:
    r2_set = para_val;
    break;
  case 1:
    q_r = para_val/(r2_set*r2_set);
    break;
  case 2:
    m2_set = para_val;
    break;
  case 3:
    q_m = para_val/(m2_set*m2_set);
    break;
  default:
    cerr << "MPC1_PatternedW::setPara(): Invalid para_id (=" << para_id << "\n";
    exit(-1);
  }
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
				   Index& nnz_h_lag, IndexStyleEnum& index_style)
{
  // The problem described in MPC1_PatternedW.hpp has 3 variables,
  //  x0 = T, x1 = W0, x2 = A
  n = 3;

  // 1 inequality constraints: 
  // 0 <= g(x) = W-A = x[1]-x[2] <= \infty
  m = 1;
  // The following limits on variables do not consider as inequality constraints
  // in IPOPT terminology
  // max{T_{min},T_old-dT_max} <= T <= min{T_{max},T_old+dT_max}
  // max{W_{min},W_old-dW_max} <= W <= min{W_{max},W_old+dW_max}
  // 0 <= A <= infty

  // 2 nonzeros in the jacobian (all elements are non-zero)
  nnz_jac_g = 2;

  // and 2 nonzeros in the hessian of the lagrangian
  // (one in the hessian of the objective for x2,
  //  and one in the hessian of the constraints for x1)
  nnz_h_lag = 6;

  // We use the standard fortran index style for row/col entries
  index_style = C_STYLE;

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::get_bounds_info(Index n, Number* x_l, Number* x_u,
				      Index m, Number* g_l, Number* g_u)
{
  // here, the n and m we gave IPOPT in get_nlp_info are passed back to us.
  // If desired, we could assert to make sure they are what we think they are.
  assert(n == 3);
  assert(m == 1);

  // x0 = T
  x_l[0] = max(T_min,T_old-dT_max);
  x_u[0] = min(T_max,T_old+dT_max);

  // x1 = W0
  x_l[1] = max(W_min,W_old-dW_max);
  x_u[1] = min(W_max,W_old+dW_max);

  // x2 = A
  x_l[2] = A_min;
  x_u[2] = A_max;

  // we have one equality constraint, so we set the bounds on this constraint
  // to be equal (and zero).
  g_l[0] = 0.0;
  g_u[0] = 2e19;

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::get_starting_point(Index n, bool init_x, Number* x,
					 bool init_z, Number* z_L, Number* z_U,
					 Index m, bool init_lambda, Number* lambda)
{
  // Here, we assume we only have starting values for x, if you code
  // your own NLP, you can provide starting values for the others if
  // you wish.
  assert(init_x == true);
  assert(init_z == false);
  assert(init_lambda == false);

  // we initialize x in bounds, in the upper right quadrant
  x[0] = T_old;
  x[1] = W_old;
  x[2] = A_old;

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::eval_f(Index n, const Number* x, bool new_x, 
			     Number& obj_value)
{
  // return the value of the objective function
  // J = q_m*(m2_sp-m2_f)^2+q_r*(r2_sp-r2_f)^2+q_h*(h_sp-h_f)^2

  Number T = x[0];
  Number W = x[1];
  Number A = x[2];

  double r2_f;
  double m2_f;
  
  eq_predict_state(T,W,A,Freq);

  r2_f = eq_r2_f();
  m2_f = eq_m2_f();
  
  obj_value = q_r*(r2_set-r2_f)*(r2_set-r2_f)+q_m*(m2_set-m2_f)*(m2_set-m2_f);

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::eval_grad_f(Index n, const Number* x, bool new_x, 
				  Number* grad_f)
{
  // return the gradient of the objective function grad_{x} f(x)
  double T = x[0];
  double W = x[1];
  double A = x[2];

  eq_predict_state(T,W,A,Freq);

  double r2_f = eq_r2_f();
  double m2_f = eq_m2_f();

  double grad_r2_f[3];
  eq_grad_r2_f(grad_r2_f);
  
  double grad_m2_f[3];
  eq_grad_m2_f(grad_m2_f);

  double dr2_fdT = grad_r2_f[0];
  double dm2_fdT = grad_m2_f[0];

  double dr2_fdW = grad_r2_f[1];
  double dm2_fdW = grad_m2_f[1];

  double dr2_fdA = grad_r2_f[2];
  double dm2_fdA = grad_m2_f[2];

  // grad_{x0} f(x):
  grad_f[0] = -2*q_r*(r2_set-r2_f)*dr2_fdT-2*q_m*(m2_set-m2_f)*dm2_fdT;

  // grad_{x1} f(x):
  grad_f[1] = -2*q_r*(r2_set-r2_f)*dr2_fdW-2*q_m*(m2_set-m2_f)*dm2_fdW;

  // grad_{x2} f(x):
  grad_f[2] = -2*q_r*(r2_set-r2_f)*dr2_fdA-2*q_m*(m2_set-m2_f)*dm2_fdA;

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::eval_g(Index n, const Number* x, bool new_x, Index m, 
			     Number* g){
  // return the value of the constraints: g(x)
  Number W = x[1];
  Number A = x[2];

  g[0] = W-A;

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::eval_jac_g(Index n, const Number* x, bool new_x,
				 Index m, Index nele_jac, 
				 Index* iRow, Index *jCol,Number* values)
{
  if (values == NULL) {
    // return the structure of the jacobian of the constraints

    // element at 1,1: grad_{x1} g_{1}(x)
    iRow[0] = 0;    jCol[0] = 1;

    // element at 1,2: grad_{x2} g_{1}(x)
    iRow[1] = 0;    jCol[1] = 2;
  }
  else {
    // return the values of the jacobian of the constraints
    // element at 1,1: grad_{x1} g_{1}(x)
    values[0] = 1;

    // element at 1,2: grad_{x1} g_{1}(x)
    values[1] = -1;
  }

  return true;
}
// -----------------------------------------------------------------------
bool MPC1_PatternedW::eval_h(Index n, const Number* x, bool new_x,
			     Number obj_factor, 
			     Index m, const Number* lambda,bool new_lambda, 
			     Index nele_hess,
			     Index* iRow, Index* jCol, Number* values)
{
  // The hessian matrix is a 3-by-3 matrix
  if (values == NULL) {
    // return the structure. This is a symmetric matrix, fill the lower left
    // triangle only.
    
    iRow[0] = 0;    jCol[0] = 0;
    iRow[1] = 1;    jCol[1] = 0;
    iRow[2] = 1;    jCol[2] = 1;
    iRow[3] = 2;    jCol[3] = 0;
    iRow[4] = 2;    jCol[4] = 1;
    iRow[5] = 2;    jCol[5] = 2;
  }
  else {
    // return the values
    // fill the objective portion
    values[0] = 0;
    values[1] = 0;
    values[2] = 0;
    values[3] = 0;
    values[4] = 0;
    values[5] = 0;
    
    // add the portion for the first constraint, which is all zero, so do NOTHING here.
  }
  return false; // USE Quasi-Newton method
}
// -----------------------------------------------------------------------
void MPC1_PatternedW::finalize_solution(SolverReturn status,
					Index n, const Number* x, 
					const Number* z_L, const Number* z_U,
					Index m, const Number* g, const Number* lambda,
					Number obj_value,
					const IpoptData* ip_data,
					IpoptCalculatedQuantities* ip_cq)
{
  // Save the current solution, which will be used as the initial solution.

  // If time == t_end, the result is discard, keep the latest result.
  if(time!=t_end){
    T_old = x[0];
    W_old = x[1];
    A_old = x[2];
  }

  double r2_f;
  double m2_f;

  eq_predict_state(T_old,W_old,A_old,Freq);
  r2_f = eq_r2_f();
  m2_f = eq_m2_f();

  printf("*** time = %f\nT = %f, W = %f, A = %f\nr_f = %f, m2_f = %f\n",
	 time,x[0],x[1],x[2],r2_f,m2_f);
}
// -----------------------------------------------------------------------
double MPC1_PatternedW::eq_C2(double T,double W){
  return k_c*exp(k_B*T/E_c)*pow(W,-a_c)/(LatticeSize*LatticeSize);
}

double MPC1_PatternedW::eq_dC2_dT(double T,double W){
  return (k_B/E_c)*k_c*exp(k_B*T/E_c)*pow(W,-a_c)/(LatticeSize*LatticeSize);
}

double MPC1_PatternedW::eq_dC2_dW(double T,double W){
  return k_c*exp(k_B*T/E_c)*(-a_c)*pow(W,-a_c-1)/(LatticeSize*LatticeSize);
}
// -----------------------------------------------------------------------
double MPC1_PatternedW::eq_sigma2(double T,double W){
  double index = (a_t+k_t*W)*T-a_v-k_v*W;
  return pi*pi/(LatticeSize*LatticeSize)*(1+exp(index));
  //  return (T0_W0+T0_W1*W)+(T1_W0+T1_W1*W)*T+(T2_W0+T2_W1*W)*T*T+(T3_W0+T3_W1*W)*T*T*T;
}

double MPC1_PatternedW::eq_dsigma2_dT(double T,double W){
  return pi*pi*exp((a_t+k_t*W)*T-a_v-k_v*W)*(a_t+k_t*W)/(LatticeSize*LatticeSize);
  // return (T1_W0+T1_W1*W)+2*(T2_W0+T2_W1*W)*T+3*(T3_W0+T3_W1*W)*T*T;
}

double MPC1_PatternedW::eq_dsigma2_dW(double T,double W){
  return pi*pi*exp((a_t+k_t*W)*T-a_v-k_v*W)*(k_t*T-k_v)/(LatticeSize*LatticeSize);
  // return T0_W1+T1_W1*T+T2_W1*T*T+T3_W1*T*T*T;
}
// -----------------------------------------------------------------------
void MPC1_PatternedW::eq_predict_state(double T,double W,double A,double Freq){
  int    p,m,n;
  int    index;
  double w_pmn;
  double temp1,temp2,temp3,temp4;
  double lambda;

  double C2;
  double sigma2;
  double dC2_dT;
  double dC2_dW;
  double dsigma2_dT;
  double dsigma2_dW;
  double dlambda_dT;
  double dlambda_dW;
  double dcov_z_dlambda;
  double dcov_z_dsigma2;
  double dcov_z_dT;
  double dcov_z_dW;
  double dcov_z_dA;
  double dmean_z_dlambda;
  double dmean_z_dw_pmn;
  double dmean_z_dT;
  double dmean_z_dW;
  double dmean_z_dA;
  double dw_pmn_dA;
  double dw_pmn_dW;

  if(T!=T_old || W!=W_old || A!=A_old){
    C2 = eq_C2(T,W);
    sigma2 = eq_sigma2(T,W);
    dC2_dT = eq_dC2_dT(T,W);
    dC2_dW = eq_dC2_dW(T,W);
    dsigma2_dT = eq_dsigma2_dT(T,W);
    dsigma2_dW = eq_dsigma2_dW(T,W);
    for(m=0;m<=mode;m++){
      for(n=0;n<=mode;n++){
	if(!(m==0&&n==0)){
	  index = index2D(m,n);
	  lambda = -4.0*C2*(m*m+n*n)*pi*pi/(XYmax*XYmax);
	  temp1 = exp(2*lambda*dt);	  
	  temp2 = sigma2*(temp1-1)/(2*lambda);
	  temp3 = exp(lambda*dt);
	  temp4 = (temp3-1)/lambda;

	  dlambda_dT = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax)*dC2_dT;
	  dlambda_dW = -4*pi*pi*(m*m+n*n)/(XYmax*XYmax)*dC2_dW;


	  for(p=0;p<4;p++){
	    w_pmn = eq_W_pmn(p,m,n,W,A,Freq);
	    // Variance ----------------------------------------------------
	    cov_z_p[p][index] = cov_z_m[p][index]*temp1+temp2;
	    // Mean value --------------------------------------------------
	    mean_z_p[p][index] = mean_z_m[p][index]*temp3+w_pmn*(temp3-1)/lambda;
	    // Mean value of state square ----------------------------------
	    
	    // Gradient of mean value of state square ----------------------
	    dcov_z_dlambda = 2*temp1*cov_z_m[p][index]*dt
	      +sigma2*(4*lambda*temp1*dt-2*(temp1-1))/(4*lambda*lambda);
	    dcov_z_dsigma2 = (temp1-1)/(2*lambda);
	    dcov_z_dT = dcov_z_dlambda*dlambda_dT+dcov_z_dsigma2*dsigma2_dT;
	    dcov_z_dW = dcov_z_dlambda*dlambda_dW+dcov_z_dsigma2*dsigma2_dW;
	    dcov_z_dA = 0.0;
	    
	    if(p==1&&m==0&&n==0){ 
	      dw_pmn_dW = XYmax;
	    }
	    else{
	      dw_pmn_dW = 0.0;
	    }

	    dmean_z_dlambda = temp3*mean_z_m[p][index]*dt*dlambda_dT
	      +w_pmn*(lambda*temp3*dt-(temp3-1))/(lambda*lambda);
	    dmean_z_dw_pmn  = (temp3-1)/lambda;
	    dw_pmn_dA = eq_dW_pmn_dA(p,m,n,Freq);
	    dmean_z_dT = dmean_z_dlambda*dlambda_dT;
	    dmean_z_dW = dmean_z_dlambda*dlambda_dW+dmean_z_dw_pmn*dw_pmn_dW;
	    dmean_z_dA = dmean_z_dw_pmn*dw_pmn_dA;

	    dz_square_dT[p][index] = dcov_z_dT+2*mean_z_p[p][index]*dmean_z_dT;
	    dz_square_dW[p][index] = dcov_z_dW+2*mean_z_p[p][index]*dmean_z_dW;
	    dz_square_dA[p][index] = dcov_z_dA+2*mean_z_p[p][index]*dmean_z_dA;
	  }
	}
      }
    }
    T_old = T;
    W_old = W;
    A_old = A;
  }
  
}
// -----------------------------------------------------------------------------
double MPC1_PatternedW::eq_r2_f(){
  int p,m,n;
  double r2_f = 0.0;
  int    index;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = index2D(m,n);
	for(p=0;p<4;p++){
	  r2_f += cov_z_p[p][index]+mean_z_p[p][index]*mean_z_p[p][index];
	}
      }
    }
  }
  return r2_f/=(XYmax*XYmax);
}

void MPC1_PatternedW::eq_grad_r2_f(double *grad_r2){
  int p,m,n;
  double dr2_fdT = 0.0;
  double dr2_fdW = 0.0;
  double dr2_fdA = 0.0;
  int    index;

  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = index2D(m,n);
	for(p=0;p<4;p++){
	  dr2_fdT += dz_square_dT[p][index];
	  dr2_fdW += dz_square_dW[p][index];
	  dr2_fdA += dz_square_dA[p][index];
	}
      }
    }
  }
  dr2_fdT /= (XYmax*XYmax);
  dr2_fdW /= (XYmax*XYmax);
  dr2_fdA /= (XYmax*XYmax);

  grad_r2[0] = dr2_fdT;
  grad_r2[1] = dr2_fdW;
  grad_r2[2] = dr2_fdA;
}
// -----------------------------------------------------------------------
double MPC1_PatternedW::eq_m2_f(){
  int p,m,n;
  double m2_f = 0.0;
  int    index;
  double K;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = index2D(m,n);
	for(p=0;p<4;p++){
	  switch(p){
	  case 0:
	    K = Km1[m][n];break;
	  case 1:
	    K = Km2[m][n];break;
	  case 2:
	    K = Km3[m][n];break;
	  case 3:
	    K = Km4[m][n];break;
	  }
	  m2_f += K*(cov_z_p[p][index]+mean_z_p[p][index]*mean_z_p[p][index]);
	}
      }
    }
  }
  return m2_f;
}

void MPC1_PatternedW::eq_grad_m2_f(double *grad_m2){
  int p,m,n;
  double dm2_fdT = 0.0;
  double dm2_fdW = 0.0;
  double dm2_fdA = 0.0;
  int    index;
  double K;
  for(m=0;m<=mode;m++){
    for(n=0;n<=mode;n++){
      if(m!=0||n!=0){
	index = index2D(m,n);
	for(p=0;p<4;p++){
	  switch(p){
	  case 0:
	    K = Km1[m][n]; break;
	  case 1:
	    K = Km2[m][n]; break;
	  case 2:
	    K = Km3[m][n]; break;
	  case 3:
	    K = Km4[m][n]; break;
	  }
	  dm2_fdT += K*dz_square_dT[p][index];
	  dm2_fdW += K*dz_square_dW[p][index];
	  dm2_fdA += K*dz_square_dA[p][index];
	}
      }
    }
  }
  grad_m2[0] = dm2_fdT;
  grad_m2[1] = dm2_fdW;
  grad_m2[2] = dm2_fdA;
}
// -----------------------------------------------------------------------
double MPC1_PatternedW::eq_W_pmn(int p,int m,int n,double W,double A, double Freq){
  double w_pmn;
  switch(p){
  case 0:
    w_pmn = 0;
    break;
  case 1:
    if(m!=0 && n!=0){
      w_pmn = 0;
    }
    else if(m==0 && n==0){
      w_pmn = W*XYmax+A*XYmax/(2*Freq*pi)*(1-cos(2*Freq*pi));
    }
    else if(m!=0 && n==0){
      if(m==Freq){
	w_pmn = 0;
      }
      else{
	w_pmn = SQRT_TWO*A*XYmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
      }
    }
    break;
  case 2:
    if(m!=0&&n==0){
      if(m==Freq){
	w_pmn = SQRT_TWO*A*XYmax/2;
      }
      else{
	w_pmn = SQRT_TWO*XYmax*A*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
      }
    }
    else{
      w_pmn = 0;
    }
    break;
  case 3:
    w_pmn= 0;
    break;
  }
  return w_pmn;
}
// -----------------------------------------------------------------------
double MPC1_PatternedW::eq_dW_pmn_dA(int p,int m,int n,double Freq){
  double dW_dA;
  switch(p){
  case 0:
    dW_dA = 0; 
    break;
  case 1:
    if(m==0&&n==0){
      dW_dA = XYmax*(1-cos(2*Freq*pi))/(2*Freq*pi);
    }
    else if(m!=Freq && n==0){
      dW_dA = SQRT_TWO*XYmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
    }
    else{
      dW_dA = 0;
    }
    break;
  case 2:
    if(m==Freq && n==0){
      dW_dA = SQRT_TWO*XYmax/2;
    }
    else if(m!=Freq && n==0){
      dW_dA = SQRT_TWO*XYmax*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
    }
    else{
      dW_dA = 0;
    }
    break;
  case 3:
    dW_dA = 0; 
    break;
  }
  return dW_dA;
}

// -----------------------------------------------------------------------
void MPC1_PatternedW::get_solution(double *x){
  x[0] = T_old;
  x[1] = W_old;
  x[2] = A_old;
  x[3] = Freq;
}
// -----------------------------------------------------------------------
